import java.util.Arrays;
import java.util.List;


public class PathPlanner {

	int diffx;
	int diffy;

	int priority[] = new int[4];

	boolean available[] = new boolean[4];

	public void findPrincess(int doorNumber) throws InterruptedException{
		while(doorNumber > 0){
			if(Data.currentFloor.isForwardAvailable(Robot.x, Robot.y, Constants.RIGHT)){
				doorNumber--;
			}

			if(doorNumber > 0){
				Robot.motionUpdate();
				
			}
		}

		Robot.rotateTo(Constants.RIGHT);
		int doorEnteranceX = Robot.x;
		int doorEnteranceY = Robot.y;
		
		Robot.motionUpdate();
		Data.currentFloor.grids[Robot.x][Robot.y].seen += 1;
		goTo(doorEnteranceX, doorEnteranceY, false);
		
		Robot.rotateTo(Constants.DOWN);
		while(Robot.y < Data.currentFloor.yGrids - 1)
			Robot.motionUpdate();

	}
	
	public void generateKingsPath(boolean sameDir){
		if(sameDir){
			for(int i=0; i<Data.kingSeenMove; i++){
				Data.kingsPath.add(new Move(Data.moveList.get(i).direction));
			}
		}
		else
			generatePath(Data.kingsPath, Data.kingSeenMove);
	}
	
	public void generatePath(List<Move> moves, int index){
		for(int i = Data.moveList.size() - 1; i >= index; i--) {
			int direction = Data.moveList.get(i).direction;
			
			if(direction == Constants.UP)
				direction = Constants.DOWN;
			
			else if(direction == Constants.DOWN)
				direction = Constants.UP;

			else if(direction == Constants.RIGHT)
				direction = Constants.LEFT;
			
			else if(direction == Constants.LEFT)
				direction = Constants.RIGHT;
			
			Move m = new Move(direction);
			moves.add(m);
		}	
	}
	
	
	//heuristic probability for deciding which room to enter
	public double probability(){
		int sureNumber = (int) (Constants.LIAR_RATE * Constants.NUM_PEOPLE) +1;
		int num1 = sureNumber - Data.saysFirst;
		int num2 = sureNumber - Data.saysSecond;
		
		if(num1 <= 0 || num2 <= 0)
			return 1;
		else
			return (double) Math.max(num1, num2) / (num1 + num2);
		
	}

	public void useGeneratedPath(List<Move> moves){
		for(int i=0; i<moves.size(); i++){
			Move move = moves.get(i);
			Robot.rotateTo(move.direction);
			Robot.motionUpdate();
		}
	}

	public void goTo(int x, int y, boolean forKing) throws InterruptedException{
		diffx = x - Robot.x;
		diffy = y - Robot.y;

		for(int i=0; i<4; i++)
			available[i]=true;

		while(Math.abs(diffx) + Math.abs(diffy) != 0){
			Thread.sleep(500);
			
			if(Data.currentFloor.grids[x][y].closed)
				break;
			
			samplePriority();
			sampleAvailability();
			if(available[priority[0]]){
				tryAndGoToDirection(priority[0], x, y);
			}
			else if(available[priority[1]]){
				tryAndGoToDirection(priority[1], x, y);
			}
			else if(available[priority[2]]){
				tryAndGoToDirection(priority[2], x, y);
			}
			else if(available[priority[3]]){
				tryAndGoToDirection(priority[3], x, y);
			}


			if(forKing && Data.kingSeen)
				break;
		}

		if(!forKing)
			Data.currentFloor.adjustSeenValues();
		//for(int i=0; i<Data.moveList.size(); i++)
			//System.out.println("Move "+i+", length="+Data.moveList.get(i).length+", vertical="+Data.moveList.get(i).vertical);
	}

	public void samplePriority(){
		int knownPenalty = 20;
		double rotations[] = new double[4];
		rotations[0] = diffx + 0.1 * Constants.RIGHT - isNextGridSeenBefore(Constants.RIGHT)*knownPenalty;
		rotations[1] = diffy + 0.1 * Constants.DOWN - isNextGridSeenBefore(Constants.DOWN)*knownPenalty;
		rotations[2] = -diffx + 0.1 * Constants.LEFT - isNextGridSeenBefore(Constants.LEFT)*knownPenalty;
		rotations[3] = -diffy + 0.1 * Constants.UP - isNextGridSeenBefore(Constants.UP)*knownPenalty;

		Arrays.sort(rotations);

		for(int i=0; i<4; i++){
			double rotation = rotations[i];
			int dir = (int)Math.abs((10*rotation-10*Math.floor(rotation)));
			priority[3-i] = dir;
		}

	}
	
	public void sampleAvailability(){
		for(int i=0; i<4; i++)
			available[i] = Data.currentFloor.isForwardAvailable(Robot.x, Robot.y, i);
	}
	
	public int isNextGridSeenBefore(int direction) {
		
		try {
			if(direction == Constants.RIGHT)
				return Data.currentFloor.grids[Robot.x + 1][Robot.y].seen;
			
			else if(direction == Constants.DOWN)
				return Data.currentFloor.grids[Robot.x][Robot.y + 1].seen;
			
			else if(direction == Constants.LEFT)
				return Data.currentFloor.grids[Robot.x - 1][Robot.y].seen;
			
			else if(direction == Constants.UP)
				return Data.currentFloor.grids[Robot.x][Robot.y - 1].seen;
		} catch(Exception e) {
			return 0;
		}
		return 0;
	}


	public void tryAndGoToDirection(int i, int x, int y){
		Robot.rotateTo(i);
		if(Data.currentFloor.isForwardAvailable(Robot.x, Robot.y, Robot.direction)){
			Robot.motionUpdate();	
			detectLoop();
//			System.out.println(Robot.x + " " + Robot.y + " " + Robot.direction * 90);
			diffx = x - Robot.x;
			diffy = y - Robot.y;

		}

	}


	public void detectLoop(){
		int x = 0, y = 0; 
		int i;
		Move move;

		search:
			for(i = Data.moveList.size() - 1; i >= 0; i--){
				move = Data.moveList.get(i);
				if(move.direction == Constants.UP)
					y--;
				
				else if(move.direction == Constants.DOWN)
					y++;

				else if(move.direction == Constants.RIGHT)
					x++;
				
				else if(move.direction == Constants.LEFT)
					x--;
				
				if(Math.abs(x) + Math.abs(y) == 0)
					break search;
			}

		if(Math.abs(x) + Math.abs(y) == 0) {
	//		System.out.println("Loop detected at move " + i);
			fixDeadlock(i);
		}

	}

	public void fixDeadlock(int moveNum){
		int x = 0, y = 0; 
		int bigNumber = 100;
		Move move;
		for(int i = Data.moveList.size() - 1; i >= moveNum; i--){
			move = Data.moveList.get(i);
			Data.currentFloor.grids[Robot.x - x][Robot.y - y].seen += bigNumber;
			
			if(move.direction == Constants.UP)
				y--;
			
			else if(move.direction == Constants.DOWN)
				y++;

			else if(move.direction == Constants.RIGHT)
				x++;
			
			else if(move.direction == Constants.LEFT)
				x--;

			if(moveNum > Data.kingSeenMove)
				Data.moveList.remove(i);
		}	

	}
	
	public void goToClosestUnseenGrid() throws InterruptedException{
		int x = 0, y = 0;
		int minDist = 1000; //some big number
		for(int i=0; i<Data.currentFloor.xGrids; i++){
			for(int j=0; j<Data.currentFloor.yGrids; j++){
				//vertical search first
				int distance = 2 * Math.abs(Robot.x - i) + Math.abs(Robot.y - j);
				if(!Data.currentFloor.grids[i][j].closed && Data.currentFloor.grids[i][j].seen==0 && distance < minDist){
					minDist = distance;
					x = i;
					y = j;
				}
			}
		}
//		System.out.println(Robot.x +" "+ Robot.y + "---" + x +" "+y);
		goTo(x, y, true);
			
	}

}
